/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
#include <stdlib.h>
#include "main.h"
#include "acb_parm.h"

static void CalcAdaptConv(
float	pc_imp[],
int	len_trunc,
float	Exc[],
int	Exc_len,
float	conv[]);

static void EndCorrectAdapt(
float	conv[],
float	Exc[],
int	Exc_len,
float	pc_imp[],
int	len_trunc);

static void ReplicateShortToFull(
int	Exc_len,
int	delay,
float	Conv[],
float	FullConv[]);

static void CalcCorEngAdapt(
float	y2[MAX_A_LEN],
float	residual[RES_LEN],
int	ex_len,
float	*cor,
float	*eng);

static void CompGainErrAdapt(
float	correlation,
float	energy,
float	*gain,
float	*match);

/*

*************************************************************************
*
* ROUTINE
*		CalcACBParms
*
* FUNCTION
*		Find ACB gain and error (match score)
*
* SYNOPSIS
*               CalcACBParms(Exc, Exc_len, pc_imp, first, delay, trunc_len, 
*			residual, gain, match)
*
*   formal 
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       Exc		float	i	excitation vector
*       Exc_len		int	i	size of ex
*	pc_imp		float	i	impulse response of PCs
*       first		int	i	first call flag
*	delay		int	i	pitch lag
*	trunc_len	int	i	length to truncate impulse response
*	residual	float	i	residual from LPC analysis
*       match		float	o	negative partial squared error
*	gain		float	o	optimal gain for excitation vector
*
*==========================================================================
*	
* DESCRIPTION
*
* (The calculations below may be valid for version 3.2, but may not be 
*  correct for version 3.3).
*
*	For each delay:
*	   a.  Filter excitation vector through truncated
*	       impulse response of perceptual weighting filter
*	       (LPC filter with bandwidth broadening).
*	   b.  Correlate filtered result with residual
*	   c.  Compute first order pitch filter coefficient (gain)
*	       and error (match) for each lag.
*
*	Note:  Proper selection of the convolution length depends on
*	       the perceptual weighting filter's expansion factor (gamma)
*	       which controls the damping of the impulse response.
*
*	        This is one of CELP's most computationally intensive
*	        routines.  Neglecting overhead, the approximate number of
*		DSP instructions (add, multiply, multiply accumulate, or
*		compare) per second (IPS) is:
*
*	        C:  convolution (recursive truncated end-point correction)
*               C': convolution (recursive truncated end-point correction)
*               R = E:  full correlation & energy
*               R'= E': delta correlation & energy
*               G:      gain quantization
*               G':     delta gain quantization
*
*	        IPS = 2.34 M (for integer delays)
*
*	ACB search complexity for integer delays:
*	implicit undefined(a-z)
*
*	DSP chip instructions/operation:
*	integer MUL, ADD, SUB, MAC, MAD, CMP
*	parameter (MUL=1)	!multiply
*	parameter (ADD=1)	!add
*	parameter (SUB=1)	!subtract
*	parameter (MAC=1)	!multiply & accumulate
*	parameter (MAD=2)	!multiply & add
*	parameter (CMP=1)	!compare
*
*	CELP algorithm parameters:
*	integer L, len, K, Kp, Np, dmin
*	real F
*	parameter (L=60)	!subframe length
*	parameter (len=30)	!length to truncate calculations (<= L)
*	parameter (K=2)		!number of full search subframes
*	parameter (Kp=2)	!number of delta search subframes
*	parameter (F=30.e-3)	!time (seconds)/frame
*	parameter (Np=32)	!number of delta subframe delays
*	parameter (dmin=20)	!minimum delay
*
*	integer j
*	parameter (j=4)
*	integer N(j), i
*	real C, R, E, G, Cp, Rp, Ep, Gp, IPS
*	data N/32, 64, 128, 256/
*	print 1
*1	format(10x,'N',10x,'C',13x,'R',12x,'E',15x,'G',13x,'MIPS')
*	do i = 1, j
*	   C = (len*(len+1)/2 + len*len)*MAC + (N(i)-1)*len*MAD
C     &	     + min(L-dmin,N(i))*(L-dmin)*ADD + (L/2-dmin)*(L-2*dmin)*ADD
C	   Cp= (len*(len+1)/2 + len*len)*MAC + (Np-1)*len*MAD
C     &	     + min(L-dmin,Np)*(L-dmin)*ADD + (L/2-dmin)*(L-2*dmin)*ADD
C	   R = N(i)*L*MAC
C	   Rp= Np*L*MAC
C	   E = R
C	   Ep= Rp
C	   G = N(i)*(1*CMP+1*MUL + 1*MUL)
C	   Gp= Np*(1*CMP+1*MUL + 1*MUL)
C	   IPS = ((C+R+E+G)*K + (Cp+Rp+Ep+Gp)*Kp)/F
C	   print *,N(i),C*K/1.e6/F,R*K/1.e6/F,E*K/1.e6/F,G*K/1.e6/F,IPS/1.e6
C	   print *,Np,Cp*Kp/1.e6/F,Rp*Kp/1.e6/F,Ep*Kp/1.e6/F,Gp*Kp/1.e6/F
C	end do
C	end
C
C  N          C             R            E               G             MIPS
C  32  0.3136667      0.1280000      0.1280000      6.4000003E-03   1.152133    
C  32  0.3136667      0.1280000      0.1280000      6.4000003E-03
C  64  0.4630000      0.2560000      0.2560000      1.2800001E-02   1.563867    
C  32  0.3136667      0.1280000      0.1280000      6.4000003E-03
C 128  0.7190000      0.5120000      0.5120000      2.5600001E-02   2.344667    
C  32  0.3136667      0.1280000      0.1280000      6.4000003E-03
C 256   1.231000       1.024000       1.024000      5.1200002E-02   3.906267    
C  32  0.3136667      0.1280000      0.1280000      6.4000003E-03
C
**************************************************************************
*
*	Compute gain and error:
*	  NOTE: Actual MSPE = e0.e0 - gain(2*correlation-gain*energy)
*		since e0.e0 is independent of the code word,
*		minimizing MSPE is equivalent to maximizing:
*		     match = gain(2*correlation-gain*energy)   (1)
*		If unquantized gain is used, this simplifies:
*		     match = correlation*gain
*
*	  NOTE: Inferior results were obtained when quantized
*		gain was used in equation (1)???
*
*	  NOTE: When the delay is less than the frame length, "match"
*	        is only an approximation to the actual error.
*
*	Independent (open-loop) quantization of gain and match (index):

***************************************************************************/

void CalcACBParms(
float 	Exc[],
int	Exc_len,
float	pc_imp[],
int	first,
int	delay,
int	trunc_len,
float	residual[RES_LEN],
float	*match,
float	*gain)
{
static	float	Conv1[MAX_A_LEN];
	float 	FullConv[MAX_A_LEN];
	float	correlation, energy;

	if (first)	{

/*  Calculate and save convolution of truncated impulse response
	for first delay  */
	  CalcAdaptConv(pc_imp, trunc_len, Exc, Exc_len, Conv1);

	}
	else	{

/*  End correct the convulution sum on subsequent delays */
	  EndCorrectAdapt(Conv1, Exc, Exc_len, pc_imp, trunc_len);
	}

/*  For lags shorter than frame size, replicate the short adaptive codeword 
	to the full codeword length */
	ReplicateShortToFull(Exc_len, delay, Conv1, FullConv);

/*  Calculate Correlation and Energy */
	CalcCorEngAdapt(FullConv, residual, Exc_len, &correlation, &energy);

/*  Compute gain and match score */
	CompGainErrAdapt(correlation, energy, gain, match);



}
/*

*************************************************************************
*                                                                         
* ROUTINE
*		CalcAdaptConv
*
* FUNCTION
*		Calculate and save convolution of truncated impulse 
*		response.
* SYNOPSIS
*		CalcAdaptConv(pc_imp, len_trunc, Exc, Exc_len, conv);
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	pc_imp		float	 i	Impulse response of PCs
*	len_trunc	int	 i	Length to truncate impulse response
*       Exc		float	 i	Excitation vector
*	Exc_len		int	 i	Lenght of excitation vector
*	conv		float	 o	Truncated convolved impulse response
*
**************************************************************************
	   Calculate and save convolution of truncated (to len)
	   impulse response for first lag of t (=mmin) samples:

	        min(i, len)
	   y     =  SUM  h * ex       , where i = 1, ..., L points
	    i, t    j=1   j    i-j+1

	              h |1 2...len x x|
	   ex |L . . . 2 1|             = y(1)
	     ex |L . . . 2 1|           = y(2)
	                   :              :
	             ex |L  . . .  2 1| = y(L)
**************************************************************************/

void CalcAdaptConv(
float	pc_imp[],
int	len_trunc,
float	Exc[],
int	Exc_len,
float	conv[])
{
int	i, j;

/*  Calculate convolution */
	for(i=0;i<Exc_len;i++)	{
	  conv[i] = 0.0;
	  for(j=0; j<=min(i, len_trunc); j++)	{
	    conv[i] += pc_imp[j] * Exc[i-j];
	  }
	}

}
/*

*************************************************************************
*                                                                         
* ROUTINE
*		EndCorrectAdapt
*
* FUNCTION
*  		End correct the convulution sum 
* SYNOPSIS
	  	EndCorrectAdapt(conv, Exc, Exc_len, pc_imp, len_trunc)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	conv		float	i/o	Truncated convolved impulse response
*       Exc		float	 i	Excitation vector
*	Exc_len		int	 i	Length of excitation vector
*	pc_imp		float	 i	Impulse response of PCs
*	len_trunc	int	 i 	Length of truncated impulse response
*
**************************************************************************
*	   End correct the convolution sum on subsequent delays:
*
*	   y     =  0
*	    0, t
*	   y     =  y        + ex  * h   where i = 1, ..., L points
*	    i, m     i-1, m-1   -m    i  and   m = t+1, ..., tmax lags
*
**************************************************************************/

void EndCorrectAdapt(
float	conv[],
float	Exc[],
int	Exc_len,
float	pc_imp[],
int	len_trunc)
{
int i;

	for(i=len_trunc-1;i>=1;i--)	{
	  conv[i-1] += Exc[0] * pc_imp[i];
	}
	
	for(i=Exc_len;i>=1;i--)	{
	  conv[i] = conv[i-1];
	}

	conv[0] = Exc[0] * pc_imp[0];
}
/*

*************************************************************************
*                                                                         *
* ROUTINE
*		ReplicateShortToFull
* FUNCTION
*		Replicate the short adaptive codeword to the full codeword 
*		length 
* SYNOPSIS
*		ReplicateShortToFull(Exc_len, delay, Conv, FullConv)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	Exc_len		int	 i	Length of excitation vector
*	delay		int	 i	Pitch lag
*	Conv		float	 i	Truncated convolved impulse response
*	FullConv	float	 o	Replicated convolved impulse response
*
**************************************************************************/

void ReplicateShortToFull(
int	Exc_len,
int	delay,
float	Conv[],
float	FullConv[])
{
int 	i;

	for(i=0;i<Exc_len;i++)	{
	  FullConv[i] = Conv[i];
	}

	if (delay < Exc_len)	{
/*  Add in 2nd convolution */
	  
	  for(i=delay; i<Exc_len; i++)	{
	    FullConv[i] += Conv[i-delay];
	  }

	  if (delay < (Exc_len / 2))	{
/*  Add in 3rd convolution */
	  
	    for(i=2*delay; i< Exc_len; i++)	{
	      FullConv[i] += Conv[i-2*delay];
	    }

	  }

	}

}
/*

*************************************************************************
*                                                                         *
* ROUTINE
*		CalcCorEngAdapt
* FUNCTION
*  		Calculate Correlation and Energy 
* SYNOPSIS
*		CalcCorEngAdapt(AnaSignal, MatchSignal, length, 
*			correlation, energy)
*
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	AnaSignal	float	 i	Signal to analyze
*	MatchSignal	float	 i	Signal to match
*	length		int	 i	Length of signals
*	correlation	float	 o	Correlation between to signals
*	energy		float	 o	Energy in signal to analyze
*
*
**************************************************************************/

void CalcCorEngAdapt(
float	AnaSignal[],
float	MatchSignal[],
int	length,
float	*correlation,
float	*energy)
{
int 	i;

	*correlation = 0.0;
	*energy = 0.0;

	for(i=0;i<length;i++)	{
	  *correlation += AnaSignal[i] * MatchSignal[i];
	  *energy += AnaSignal[i] * AnaSignal[i];
	}
}
/*

*************************************************************************
*                                                                         *
* ROUTINE
*		CompGainErrAdapt
* FUNCTION
*  		Compute gain and error 
* SYNOPSIS
*		CompGainErrAdapt(correlation, energy, gain, match)
*
*
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	correlation	float	 i	Correlation
*	energy		float	 i	Energy
*	gain		float	 o	Adaptive Codebook gain
*	match		float	 o	Negative partial squared error
*
*
**************************************************************************/

void CompGainErrAdapt(
float	correlation,
float	energy,
float	*gain,
float	*match)
{

	if(energy <= 0)
	  energy = 1.0;

	*gain = correlation / energy;

	*match = correlation * *gain;
}
